#include "HT03_driver.h"
#include "interrupt_manager.h"
#include <stdlib.h>

// 电机极限PID
#define P_MIN -95.5f // Radians
#define P_MAX 95.5f
#define V_MIN -45.0f // Rad/s
#define V_MAX 45.0f
#define KP_MIN 0.0f // N-m/rad
#define KP_MAX 500.0f
#define KD_MIN 0.0f // N-m/rad/s
#define KD_MAX 5.0f
#define T_MIN -18.0f
#define T_MAX 18.0f

// #define LIMIT_MIN_MAX(x, min, max) (x) = (((x) <= (min)) ? (min) : (((x) >= (max)) ? (max) : (x)))

/* HT电机模式代码 */
enum HT_Commands
{
    CMD_ENTER_MOTOR_MODE = 0xFC,
    CMD_EXIT_MOTOR_MODE = 0xFD,
    CMD_SET_MOTOR_ZEROPOSITION = 0xFE
};

/**
 * 映射设置的float参数到HT电机适配参数
 * 16 bit position command
 * 12 bit velocity command
 * 12 bit Kp
 * 12 bit Kd
 * 12 bit Feed-Forward Current 前馈电流
 */
static inline uint16_t float_to_uint(float x, float x_min, float x_max, uint8_t bits)
{
    return (uint16_t)((x - x_min) * ((float)((1 << bits) - 1)) / (x_max - x_min));
}

/**
 * CAN接收数据映射到float类型
 */
static inline float uint_to_float(int x_int, float x_min, float x_max, int bits)
{
    return ((float)x_int) * (x_max - x_min) / ((float)((1 << bits) - 1)) + x_min;
}

static HT_t **MotorsArray;  // 电机指针数组
static uint8_t MotorsCount; // 电机数量

// CAN中断回调函数
void HT_can_callback(CAN_RxHeaderTypeDef *pHeader, uint8_t *pBuf)
{
    if (pHeader->StdId == 0x00)
    {
        for (uint8_t i = 0; i < MotorsCount; i++)
        {
            if (MotorsArray[i]->id == pBuf[0])
            {
                // 接收数据转换
                MotorsArray[i]->position = uint_to_float((uint16_t)(pBuf[1] << 8 | pBuf[2]), P_MIN, P_MAX, 16);
                MotorsArray[i]->velocity = uint_to_float((uint16_t)(pBuf[3] << 4 | pBuf[4] >> 4), V_MIN, V_MAX, 12);
                MotorsArray[i]->torque = uint_to_float((uint16_t)((pBuf[4] & 0x0F) << 8 | pBuf[5]), T_MIN, T_MAX, 12);
            }
        }
    }
}

static uint8_t cmd[8] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x00};

// 电机初始化,不可以重复初始化
void HT_init(HT_t *motor, uint8_t id, CAN_HandleTypeDef *can)
{
    motor->can = can;
    MotorsCount++; // 电机数+1
    MotorsArray = (HT_t **)realloc(MotorsArray, MotorsCount * sizeof(HT_t *));
    MotorsArray[MotorsCount - 1] = motor;
    motor->id = id;
    motor->target_kp = 0.0;
    motor->target_kd = 0.0;
    motor->target_torque = 0.0;
    motor->target_velocity = 0.0;
    motor->target_position = 0.0;

    // 进入电机模式
    cmd[7] = CMD_ENTER_MOTOR_MODE;
    can_transmit(can, id, cmd);
    HAL_Delay(1);
    // 设置零参数
    HT_send_parameter(motor, 0, 0, 0, 0, 0);
    HAL_Delay(1);
    // 设置电机零位置
    cmd[7] = CMD_SET_MOTOR_ZEROPOSITION;
    can_transmit(can, id, cmd);
    HAL_Delay(1);
}

inline void HT_run(HT_t *motor)
{
    HT_send_parameter(motor, motor->target_torque, motor->target_velocity, motor->target_position, motor->target_kp, motor->target_kd);
}

// 设置位置模式, 顺时针为正, rad
inline void HT_set_position(HT_t *motor, float angle, float kp, float kd, float torque)
{
    motor->target_torque = torque;
    motor->target_velocity = 0;
    motor->target_position = angle;
    motor->target_kp = kp;
    motor->target_kd = kd;
}

// 设置速度模式, 顺时针为正, rad/s
inline void HT_set_speed(HT_t *motor, float speed, float kd)
{
    motor->target_torque = 0;
    motor->target_velocity = speed;
    motor->target_position = 0;
    motor->target_kp = 0;
    motor->target_kd = kd;
}

// 设置力矩模式, N*M
inline void HT_set_torque(HT_t *motor, float torque)
{
    motor->target_torque = torque;
    motor->target_velocity = 0;
    motor->target_position = 0;
    motor->target_kp = 0;
    motor->target_kd = 0;
}

// 设置阻尼模式
inline void HT_set_damp(HT_t *motor, float kd)
{
    motor->target_torque = 0;
    motor->target_velocity = 0;
    motor->target_position = 0;
    motor->target_kp = 0;
    motor->target_kd = kd;
}

// 设置零力矩模式
inline void HT_set_zero_torque(HT_t *motor)
{
    motor->target_torque = 0;
    motor->target_velocity = 0;
    motor->target_position = 0;
    motor->target_kp = 0;
    motor->target_kd = 0;
}

// 设置电机参数
inline void HT_send_parameter(HT_t *motor, float torque, float speed, float angle, float kp, float kd)
{
    uint16_t byte_p, byte_v, byte_kp, byte_kd, byte_t;
    uint8_t buf[8];

    /* 根据协议，对float参数进行转换 */
    byte_p = float_to_uint(angle, P_MIN, P_MAX, 16);
    byte_v = float_to_uint(speed, V_MIN, V_MAX, 12);
    byte_kp = float_to_uint(kp, KP_MIN, KP_MAX, 12);
    byte_kd = float_to_uint(kd, KD_MIN, KD_MAX, 12);
    byte_t = float_to_uint(torque, T_MIN, T_MAX, 12);

    /* 根据传输协议，把数据转换为CAN命令数据字段 */
    buf[0] = byte_p >> 8;
    buf[1] = byte_p & 0xFF;
    buf[2] = byte_v >> 4;
    buf[3] = ((byte_v & 0xF) << 4) | (byte_kp >> 8);
    buf[4] = byte_kp & 0xFF;
    buf[5] = byte_kd >> 4;
    buf[6] = ((byte_kd & 0xF) << 4) | (byte_t >> 8);
    buf[7] = byte_t & 0xff;

    /* 如果CAN邮箱空闲 */
    while (HAL_CAN_GetTxMailboxesFreeLevel(motor->can) == 0)
        continue;
    can_transmit(motor->can, motor->id, buf);
}

// 设置电机零位置
inline void HT_set_zero_position(HT_t *motor)
{
    cmd[7] = CMD_SET_MOTOR_ZEROPOSITION;
    can_transmit(motor->can, motor->id, cmd);
}

inline uint8_t HT_arrival_position(HT_t *motor, float per)
{
    float temp = motor->position - motor->target_position;

    /* 取绝对值 */
    if (temp < 0)
    {
        temp = -temp;
    }

    if (temp < per)
    {
        return 1;
    }

    return 0;
}
